#include "parser.h"

#define ERROR	0
#define OK		1

long int xvalue;
long int yvalue;

int parseParameters(const char * command);



/*
 * SUPORTED COMMANDS:
 *
 * PD Baja la pluma y se dirige a la coordenada X,Y
 * PU Levanta la pluma y se dirige a la coordenada X,Y
 * IN Inicializa la maquina
 * SP Selecciona la pluma
 * PA Mueve la pluma a la posicion indicada (absoluta)
 * PR Mueve la pluma desde la posicion actual (relativo)
 *
 */

void executeCommand (const char* command) {

	char buffer[10];

	if (strncmp(command,"PU",2) == 0) {

		command = command + 2;
		if (parseParameters(command) == OK) {
			//putpixel(xvalue, yvalue, 0);
			penUp();
			line_3D(X.Pos, Y.Pos, 0, xvalue, yvalue, 0);
			USART_Send_String(OK_MSG);
		}
		else {
			USART_Send_String(ERR_MSG);
		}
	}
	else if (strncmp(command,"PD",2) == 0) {

		command = command + 2;
		if (parseParameters(command) == OK) {
			//putpixel(xvalue, yvalue, 0);
			penDown();
			line_3D(X.Pos, Y.Pos, 0, xvalue, yvalue, 0);
			USART_Send_String(OK_MSG);
		}
		else {
			USART_Send_String(ERR_MSG);
		}
	}
	else if (strncmp(command,"PA",2) == 0) {

	}
	else if (strncmp(command,"PR",2) == 0) {

	}
	else if (strncmp(command,"IN",2) == 0) {

		// Initialize the plotter.
		// Goes to position 0,0 to start drawing.
		initialize();
		USART_Send_String(OK_MSG);
	}
	else if (strncmp(command,"SP",2) == 0) {

		// Select pen.
		// We only have one pen!
		USART_Send_String(OK_MSG);
	}
	else if (strncmp(command,"GP",2) == 0) {

		// Sends the properties of the device.
		itoa(X.maxSteps, buffer, 10);
		USART_Send_String(buffer);
		USART_Transmit('\n');
		itoa(Y.maxSteps,buffer,10);
		USART_Send_String(buffer);
		USART_Transmit('\n');
	}
	else {
		// Error!!
	}
}

int parseParameters(const char * command) {

	char *nextparam, *lastpart;

	// Se calcula el primer parametro.
	xvalue = strtol(command, &nextparam, 10);

	if (nextparam == NULL || *nextparam != ','){
		return ERROR;
	}

	nextparam++;

	// Se calcula el segundo parametro;
	yvalue = strtol(nextparam, &lastpart, 10);

	// Se acepta solo 2 parametros.
	if (lastpart == NULL || *lastpart != ';') {
		return ERROR;
	}

	return OK;
}
